%%
%% load images and match files for the first example
%%
name_base = 'library';
I1 = imread(strcat(name_base,'1.jpg'),'jpg');
I2 = imread(strcat(name_base,'2.jpg'),'jpg');
matches = load(strcat(name_base,'_matches.txt')); 
% this is a N x 4 file where the first two numbers of each row
% are coordinates of corners in the first image and the last two
% are coordinates of corresponding corners in the second image: 
% matches(i,1:2) is a point in the first image
% matches(i,3:4) is a corresponding point in the second image

N = size(matches,1);

%%
%% display two images side-by-side with matches
%% this code is to help you visualize the matches, you don't need
%% to use it to produce the results for the assignment
%%
% imshow([I1 I2]); hold on;
% plot(matches(:,1), matches(:,2), '+r');
% plot(matches(:,3)+size(I1,2), matches(:,4), '+r');
% line([matches(:,1) matches(:,3) + size(I1,2)]', matches(:,[2 4])', 'Color', 'r');

%%
%% display second image with epipolar lines reprojected 
%% from the first image
%%

% first, fit fundamental matrix to the matches
F = fit_fundamental(matches,false); % this is a function that you should write
L = [matches(:,1:2) ones(N,1)] * F; % transform points from 
% the first image to get epipolar lines in the second image

% find points on epipolar lines L closest to matches(:,3:4)
L = L ./ repmat(sqrt(L(:,1).^2 + L(:,2).^2), 1, 3); % rescale the line
pt_line_dist = sum(L .* [matches(:,3:4) ones(N,1)],2);
closest_pt = matches(:,3:4) - L(:,1:2) .* repmat(pt_line_dist, 1, 2);
distance2=mean(sqrt(sum((matches(:,3:4)-closest_pt).^2)));

% find endpoints of segment on epipolar line (for display purposes)
pt1 = closest_pt - [L(:,2) -L(:,1)] * 10; % offset from the closest point is 10 pixels
pt2 = closest_pt + [L(:,2) -L(:,1)] * 10;

% display points and segments of corresponding epipolar lines
% figure;
% imshow(I2); hold on;
% plot(matches(:,3), matches(:,4), '+r');
% line([matches(:,3) closest_pt(:,1)]', [matches(:,4) closest_pt(:,2)]', 'Color', 'r');
% line([pt1(:,1) pt2(:,1)]', [pt1(:,2) pt2(:,2)]', 'Color', 'g');

camera1=load(strcat(name_base,'1_camera.txt'));
camera2=load(strcat(name_base,'2_camera.txt'));
[points,points_proj1,points_proj2]=my_triangulate(matches(:,1:2),camera1,matches(:,3:4),camera2);
project_distance_1=mean(sqrt(sum((points_proj1-matches(:,1:2)).^2)));
project_distance_2=mean(sqrt(sum((points_proj2-matches(:,3:4)).^2)));
camera_center1=get_cam_center(camera1);
camera_center2=get_cam_center(camera2);

figure; axis equal;  hold on; 
plot3(-points(:,1), points(:,2), points(:,3), '*r');
plot3(-camera_center1(1), camera_center1(2), camera_center1(3), '+b');
plot3(-camera_center2(1), camera_center2(2), camera_center2(3), '+g');
grid on; xlabel('x'); ylabel('y'); zlabel('z'); axis equal;



